#! /usr/bin/env python3

import time, sys, os, json, math
import paho.mqtt.client as mqtt

sys.path.append(os.path.dirname(os.path.abspath(__file__)))
from config.config import ADDRESS, PORT, CLIENT_ID,SERIAL_NUMBER,PWD,MQTT_PUB_TOPIC,ROS_SUB_TOPIC
#from utils.color_msg import ColorMsg
class MqttPublish:  
    def __init__(self):
        self.broker_address = ADDRESS
        self.broker_port = PORT
        self.client_id = CLIENT_ID
        self.connected = False
        self.mqtt_client = mqtt.Client(client_id=self.client_id)
        self.mqtt_client.username_pw_set(SERIAL_NUMBER, PWD) 
        self.mqtt_client.on_connect = self.on_connect
        self.mqtt_client.on_message = self.on_message
        self.mqtt_client.on_disconnect = self.on_disconnect
        self.pub = None
        self.received = 0
        self.seq = 0
        print("开启MQTT发布，收到机械臂ROS的topic转发给MQTT")
  
    def on_connect(self, client, userdata, flags, rc):
        if rc == 0:
            self.connected = True
            print("Connected to MQTT Broker!")
        else:
            self.connected = False
            print("Failed to connect, return code %d\n", rc)
            # print(f"Connected with result code {str(rc)}")  
        #self.mqtt_client.subscribe(MQTT_SUB_TOPIC)
        #sub = rospy.Subscriber(ROS_SUB_TOPIC,JointState,self.toMqtt,queue_size=100)

    def on_disconnect(self, client, userdata, rc):
        # print("断开链接")
        print("Unexpected disconnection rc = " + str(rc))
        self.connected = False
        pass
   
  
    def on_message(self, client, userdata, msg):  
        self.received = self.received + 1
        print(f"Received MQTT message on topic '{msg.topic}': {self.received}") 
  
    def connect_mqtt(self):  
        self.mqtt_client.connect(self.broker_address, self.broker_port, 60)  
        self.mqtt_client.loop_start()  
  
    def disconnect_mqtt(self):
        
        self.mqtt_client.loop_stop()  
        self.mqtt_client.disconnect()
    # 发布给MQTT服务器
    def toMqtt(self):
        #print("+++++++++++++++")
        #print(data)
        if not self.connected:
            return
        
        self.seq += 1
        cur = time.time()
        tmp_dict = {
            "header": {
                "seq": self.seq,
                "stamp": {
                    "secs": math.floor(cur),
                    "nsecs": math.floor((cur - math.floor(cur)) * 1000000)
                },
                "frame_id": self.seq % 100
            },
            "name": 'robot',
            "position": '100',
            "velocity": 0.32,
            "effort": 0.21
        }
        j_m = json.dumps(tmp_dict)
        self.mqtt_client.publish(MQTT_PUB_TOPIC, j_m, qos=1)
        print('send message to mqtt, topic: ' + MQTT_PUB_TOPIC + ', seq: ' + str(self.seq))
        
    def run(self):  
        #rospy.init_node('mqtt_ros_publish', anonymous=True)   
        self.connect_mqtt()
        # 这里获取机械臂遥操的话题然后发布给mqtt
        
        # ROS节点将保持运行，直到被Ctrl+C中断  
        #rospy.spin()  
  
        #self.disconnect_mqtt()
        while True:
            self.toMqtt()
            time.sleep(0.1)
   

if __name__ == '__main__':

    mp = MqttPublish() # /1/0001/forward/post 为被控制话题
    #bridge = MqttRosBridge('42.193.121.181', 2883, '0002', '/1/0002/#')
    mp.run()  
